;---------------------------------------------------------------------------;
; USI control functions
;---------------------------------------------------------------------------;

.nolist
#include <avr/io.h>	// Include device specific definitions.
.list

;---------------------------------------------------------------------------;
; Simple Delay
;---------------------------------------------------------------------------;
; void delay_ms(WORD ms);
; void delay_us(WORD us);

.global delay_ms
.func delay_ms
delay_ms:
	wdr			; Reset WDT
	sbiw	r24, 1		; if (ms-- == 0) return;
	brcs	9f		; /
	ldi	ZL, lo8(4000)	; 1ms delay (16MHz)
	ldi	ZH, hi8(4000)	; 
1:	sbiw	ZL, 1		; 
	brne	1b		; /
	rjmp	delay_ms
9:	ret
.endfunc


.global delay_us
.func delay_us
delay_us:
	ldi	r23, 2
1:	dec	r23
	brne	1b
	sbiw	r24, 1
	brne	delay_us
	wdr
	ret
.endfunc


;---------------------------------------------------------------------------;
; Initialize USI
;
; void init_spi (void);

.global init_spi
.func init_spi
init_spi:
	;cbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 0
	;sbi	_SFR_IO_ADDR(DDRC), PC0 //CLK OUT

	;sbi _SFR_IO_ADDR(PORTC), PC1 // Pull-up activated
	;cbi _SFR_IO_ADDR(DDRC), PC1 // DI IN

	sbi	_SFR_IO_ADDR(PORTC), PC2 //DO 1
	;sbi	_SFR_IO_ADDR(DDRC), PC2 //DO OUT

	;sbi	_SFR_IO_ADDR(PORTC), PC3 //CS 1
	;sbi	_SFR_IO_ADDR(DDRC), PC3 //CS OUT
	ret
.endfunc



;---------------------------------------------------------------------------;
; Receive a byte
;
; BYTE rcv_spi (void);

.global rcv_spi
.func rcv_spi
rcv_spi:
	push r18
	push r19
	ldi r18, 0
	ldi r19, 0b00000001
	.rept 8
	lsl r18	;left shift
	sbi	_SFR_IO_ADDR(PORTC), PC0 ;CLK 1
	nop
	nop
	sbic _SFR_IO_ADDR(PINC), PC1 ;read bit
	add r18, r19
	cbi	_SFR_IO_ADDR(PORTC), PC0 ;CKL 0
	.endr
	mov	r24, r18	;copy result from shift registr r18 into return register
	pop r19
	pop r18
	ret
.endfunc



;---------------------------------------------------------------------------;
; Transmit a byte
;
; void xmit_spi (BYTE);

.global xmit_spi
.func xmit_spi
xmit_spi:
	ldi	r25, 8
1:	bst	r24, 7				;DO = data bit to be sent
	brtc 2f
	sbi	_SFR_IO_ADDR(PORTC), PC2 //DO 1
	rjmp 3f
2:
	cbi	_SFR_IO_ADDR(PORTC), PC2 //DO 0
3:
	lsl	r24				;/
	sbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 1
	nop
	cbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 0
	dec	r25				;while(--r23)
	brne	1b				;/

	sbi	_SFR_IO_ADDR(PORTC), PC2 //DO 1
	ret
.endfunc



;---------------------------------------------------------------------------;
; Read and forward a partial data block
;
; void fwd_blk_part (void*, WORD, WORD);

.global fwd_blk_part
.func fwd_blk_part
fwd_blk_part:
	movw	XL, r24			;X = R25:R24 (memory address)
	movw	ZL, r22			;Z = R23:R22 (byte offset in the sector)

	ldi	r18, lo8(514)		;R19:R18 = 514, Number of bytes to receive
	ldi	r19, hi8(514)		;/
	sub	r18, ZL			;R19:R18 -= Z
	sbc	r19, ZH			;/
	sub	r18, r20		;R19:R18 -= R21:R20
	sbc	r19, r21		;/
	; Skip leading data bytes
	;ldi	r24, 0b000100		;PB2(SCK)
1:	sbiw	ZL, 1			;Skip leading data...
	brcs	2f			;
	.rept 8			;Discard a byte on SPI
    sbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 1
	nop
	cbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 0
	.endr				;/
	rjmp	1b			;
2:	sbiw	XL, 0			;Destination?
	breq	fb_wave

fb_mem:	; Store intermediate data bytes to the memory
	rcall	rcv_spi			;do
	st	X+, r24			; *X++ = rcv_spi()
	subi	r20, 1			;while (--r21:r20)
	sbci	r21, 0			;
	brne	fb_mem			;/
	rjmp	fb_exit

fb_wave: ; Forward intermediate data bytes to the wave FIFO
	lds	r22, FifoWi		;r22 = FIFO write index

3:	ldi	XL, lo8(Buff)		;X = Buff + R22
	ldi	XH, hi8(Buff)		;
	add	XL, r22			;
	adc	XH, r1			;/
4:	lds	r24, FifoCt		;wait while FIFO full
	cpi	r24, 252		;
	brcc	4b			;/

// Single output
	rcall	rcv_spi			;
	mov	ZL, r24			;/

8:	st	X+, ZL			;Store ZL into FIFO
	cli				;
	lds	r24, FifoCt		;
	inc	r24			;
	sts	FifoCt, r24		;
	sei				;
	inc	r22			;/
//#endif
	subi	r20, lo8(1)		;while(--R21:R20)
	sbci	r21, hi8(1)		;
	brne	3b			;/
	sts	FifoWi, r22		;Save FIFO write index

fb_exit:
	;ldi	r24, 0b000100		;PB2(SCK)
9:	.rept 8			;Discard a byte on SPI
	sbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 1
	nop
	cbi	_SFR_IO_ADDR(PORTC), PC0 //CLK 0
	.endr				;/
	subi	r18, lo8(1)		;Repeat r19:r18 times
	sbci	r19, hi8(1)		;
	brne	9b			;/

	ret
.endfunc



;---------------------------------------------------------------------------;
; Read and forward the data block
;
; ISR(TIM0_COMPA_vect);


.global TIMER1_COMPA_vect
.func TIMER1_COMPA_vect
TIMER1_COMPA_vect:
	push	r24				;Save regs.
	in	r24, _SFR_IO_ADDR(SREG)		;
	push	r24				;
	push	ZL				;
	push	ZH				;/

	sbi _SFR_IO_ADDR(DDRC), PC5
	sbi _SFR_IO_ADDR(PORTC), PC5

	lds	ZL, FifoRi			;Get FIFO read index
	clr	ZH				;Z = pointer to the top of FIFO
	subi	ZL, lo8(-(Buff))		;
	sbci	ZH, hi8(-(Buff))		;/
	lds	r24, FifoCt			;Load FIFO data counter
// Single output
	subi	r24, 1				;Check availability of the sampling data
	brcs	9f				;/
	sts	FifoCt, r24			;Save FIFO data counter
	ld	r24, Z+				;Send data to PWM
	out	_SFR_IO_ADDR(OCR2), r24	;/

	subi	ZL, lo8(Buff)			;Save FIFO read index
	sts	FifoRi, ZL			;/

9:	pop	ZH				;Restore regs.
	pop	ZL				;
	pop	r24				;
	out	_SFR_IO_ADDR(SREG), r24		;
	pop	r24				;/
	reti
.endfunc

